//! \file backtracer_arm_exidx.c

#include <string.h>

#include "linux_arm_exidx_elf.h"
#include "backtracer_arm_exidx.h"
#include "debug.h"

//------------------------------------------------------------------------------

TBGError DefaultBacktraceExidx(func_map *pmap, void *frame_begin, 
                               void *curr_addr) {
    TBGError ret = 0;
    CHECK_DBG_ERR(pmap, NULL_PTR);
    bool found = false;
    int i = 0;
    for(i = 0; i < pmap->size_; ++i) {
            unsigned int name_idx = pmap->map_[i].name_idx_;
            CHECK_DBG_ERR(name_idx < pmap->size_, BIG_INDEX);
            if(!strcmp(pmap->names_[name_idx], "main")) {
                found = true;
                break;
            }
    }
    CHECK_RET(found, NO_MAIN_FUNC, exit_point, ret);
    
    const size_t tr_size = DEFAULT_TRACE_SIZE;
    trace_struct tr; //allocate!
    tr.trace_idx = calloc(tr_size, sizeof(int));
    tr.trace_addr = calloc(tr_size, sizeof(addr_t));
    CHECK_RET(tr.trace_idx, CANNOT_ALLOC_MEMORY, free_point, ret);
    CHECK_RET(tr.trace_addr, CANNOT_ALLOC_MEMORY, free_point, ret);


    CHECK_CALL(GenBacktraceExidx(pmap, frame_begin, curr_addr, 
                                (void*) pmap->map_[i].addr_, &tr, tr_size),
               free_point, ret);
    PrintBacktrace(pmap, &tr);

free_point:
    free(tr.trace_idx);
    free(tr.trace_addr);
exit_point:
    return ret;
}

//------------------------------------------------------------------------------

TBGError GenBacktraceExidx(func_map *pmap, void *frame_begin, void *curr_addr, 
                           void *stop_addr, trace_struct *stack_trace,
                           size_t trace_max_size) {
    TBGError ret = 0;
    CHECK_DBG_ERR(stack_trace, NULL_PTR);
    CHECK_DBG_ERR(stack_trace->trace_addr, NULL_PTR);
    CHECK_DBG_ERR(stack_trace->trace_idx, NULL_PTR);
    CHECK_DBG_ERR(pmap, NULL_PTR);
    CHECK_DBG_ERR(pmap->map_, NULL_PTR);

    unwind_status status = {};
    CHECK_CALL(GetRegisters(status.regs), exit_point, ret);
    
    elf_unwind_session unw_sess = {};
//    InitUnwindSession(&unw_sess);

    uint8_t buf[MAX_CMD_BUF_LENGTH] = {0};
    size_t buf_len = 0;

    size_t cur_paste_idx = 0;

    int find_res = _FindByAddr(pmap, (addr_t) curr_addr);
    
    while(cur_paste_idx < trace_max_size) {
//        printf("curr_addr: [%x], find_res: [%d]\n", curr_addr, find_res);

        stack_trace->trace_idx[cur_paste_idx] = find_res;
        stack_trace->trace_addr[cur_paste_idx++] = (addr_t) curr_addr;
        if(find_res < 0)
            break;
        CHECK_DBG_ERR(find_res < pmap->size_, BIG_INDEX);
        if(pmap->map_[find_res].addr_ == (addr_t) stop_addr)
            break;
    
        CHECK_CALL(ElfFindUnwindInfo(&unw_sess, (addr_t) curr_addr),
                   exit_point, ret);

        CHECK_CALL(ElfGetUnwindCmd(&unw_sess, (addr_t) curr_addr, buf,
                                   &buf_len),
                   exit_point, ret);
        
//        printf("cmd_buf_len: [%d]\n", buf_len);
        
        CHECK_CALL(ArmExidxDecode(&status, (unsigned int*) buf, buf_len),
                   exit_point, ret);
       
        curr_addr = (void*) status.regs[15];

        find_res = _FindByAddr(pmap, (addr_t) curr_addr );

    }

    stack_trace->trace_size = cur_paste_idx;
exit_point:
    return ret;
}

//------------------------------------------------------------------------------

TBGError ArmExidxDecode(unwind_status *status, unsigned int *buf, size_t len) {
    TBGError ret = 0;
    CHECK_DBG_ERR(status, NULL_PTR);
    CHECK_DBG_ERR(buf, NULL_PTR);
    
    arm_exidx_cmd gen = {};
    status->r15_updated = false;

    #define cmd (*i)
    unsigned int *i = 0;
    for(i = buf; i < buf + len; ++i) {
//        printf("\n");
//        int j = 0;
//        for(j = 0; j < 16; ++j)
//            printf("r[%d] = [%x]\n", j, status->regs[j]);
//        printf("\n");
        if((cmd >> 6) == 0) { // DATA_POP 00xxxxxx
            gen.command = CMD_DATA_POP;
            gen.data = (unsigned long) (cmd << 2) + 4; 
        }
        else if((cmd >> 6) == 1) { // DATA_PUSH 01xxxxxx
            gen.command = CMD_DATA_PUSH;
            gen.data = (unsigned long) ((cmd & 0x3f) << 2) + 4; 
        }
        else if((cmd >> 4) == (1 << 3)) {// 1000**** ********
            gen.data = (unsigned long) (cmd & 0xf) << 12;
            i++;
            if(cmd == 0 && gen.data == 0)  // REFUSED 10000000 00000000
                gen.command = CMD_REFUSED;
            else { // REG_POP 1000iiii iiiiiiii
                gen.command = CMD_REG_POP;
                gen.data |= (unsigned long) cmd << 4;
            }
        }
        else if((cmd >> 4) == 0x9 && cmd != 0x9D && cmd != 0x9F) {
                             // REG_TO_SP 1001nnnn, nnnn != 13,15
            gen.command = CMD_REG_TO_SP;
            gen.data = cmd & 0xf;
        }
        else if((cmd >> 3) == 0x14) { // REG_POP 10100nnn
            gen.command = CMD_REG_POP;
            int j = 0;
            for(j = 4; j <= 4 + (cmd & 0x7); ++j)
                gen.data |= (1 << j);
        }
        else if((cmd >> 3) == 0x15) { // REG_POP_R14 10101nnn
            gen.command = CMD_REG_POP;
            int j = 4;
            for(j = 4; j <= 4 + (cmd & 0x7); ++j)
                gen.data |= (1 << j);
            gen.data |= (1 << 14);
        }
        else if(cmd == ARM_FINISH_CMD_CODE) // FINISH 10110000 
            gen.command = CMD_FINISH;
        else if(cmd == 0xB1) { // 10110001 ********
            i++;
            if((cmd >> 4) == 0) { // REG_POP 10110001 000iiii
                gen.command = CMD_REG_POP;
                gen.data |= cmd;
            }
            else
                gen.command = CMD_RESERVED;
        }
        else if(cmd == 0xB2) { // DATA_POP 10110010 uleb128
            gen.command = CMD_DATA_POP;
            unsigned long shift = 0;
            while(true)
            {
                i++;
                gen.data |= ((cmd & 0x7f) << shift);
                if (cmd & 0x80)
                    break;
                shift += 7;
            }
            gen.data <<= 2;
            gen.data += 0x204;
        }
        else if(cmd == 0xB3) { // VFP_POP_X 10110011 sssscccc
            i++;
            gen.command = CMD_VFP_POP_X;
            gen.data = cmd & 0xf;

        }
        else if((cmd >> 3) == 0x17) { // VFP_POP_X 10111nnn
            gen.command = CMD_VFP_POP_X;
            gen.data = cmd & 0x7;
        }
        else if((cmd >> 3) == 0x18 && cmd != 0xC6 && cmd != 0xC7) {
                                    // WREG_POP 11000nnn (nnn != 6, 7)
            gen.command = CMD_WREG_POP;
            gen.data = cmd & 0x7;
        }
        else if(cmd == 0xC6) { // WREG_POP 11000110 sssscccc
            i++;
            gen.command = CMD_WREG_POP;
            gen.data = cmd & 0xf;
        }
        else if(cmd == 0xC7) { // 11000111
            i++;
            if(cmd != 0 && (cmd >> 4) == 0) { // WCGR_POP 11000111 0000iiii
                gen.command = CMD_WCGR_POP;
                int j = 0;
                for(j = 0; j < 4; ++j)
                    if(cmd & (1 << j))
                        gen.data++;
            } 
            else
                gen.command = CMD_RESERVED;
        }
        else if(cmd == 0xC8 || cmd == 0xC9) { // VFP_POP_D 11001000 sssscccc
            i++;
            gen.command = CMD_VFP_POP_D;
            gen.data = cmd & 0xf;
        }
        else
            gen.command = CMD_RESERVED;
        
        CHECK_CALL(ArmExidxApplyCmd(status, gen), exit_point, ret);
        if(gen.command == CMD_FINISH)
            goto exit_point;
    }
    #undef cmd

exit_point:
    return ret;
}

//------------------------------------------------------------------------------

TBGError ArmExidxApplyCmd(unwind_status *status, arm_exidx_cmd cmd) {
    TBGError ret = 0;
    int sp_updated = 0;
    CHECK_DBG_ERR(status, NULL_PTR);
    int i = 0;
    switch(cmd.command) {
        case CMD_DATA_POP:
            LogPrintf("CMD_DATA_POP of size [%x]\n", cmd.data);
            status->regs[13] += cmd.data;
            break;
        case CMD_DATA_PUSH:
            LogPrintf("CMD_DATA_PUSH\n");
            status->regs[13] -= cmd.data;
            break;
        case CMD_REFUSED:
            LogPrintf("CMD_REFUSED\n");
            CHECK_RET(false, REFUSE_UNW_COMMAND, exit_point, ret);
        case CMD_REG_POP: {
            addr_t old_sp = status->regs[13];
            LogPrintf("CMD_REG_POP\n");
            for(i = 0; i < 16; ++i) 
                if((cmd.data >> i) & 1) {
                    LogPrintf("Try to pop r[%d]\n", i);
                    ReadData(old_sp, status->regs + i, 1);
                    old_sp += sizeof(addr_t);
                    if(i == 13)
                        sp_updated = 1;
                    if(i == 15)
                        status->r15_updated = true;
                    LogPrintf("New r[%d] is [%x]\n", i, status->regs[i]);
                }
            if(!sp_updated)
                status->regs[13] = old_sp;
            break;
            }
        case CMD_REG_TO_SP:
            LogPrintf("CMD_REG_TO_SP. Old regs[13] is [%x]\n", status->regs[13]);
            CHECK_RET(cmd.data != 13 && cmd.data != 15, SP_TO_SP_UNW_COMMAND,
                      exit_point, ret);
            status->regs[13] = status->regs[cmd.data];
            LogPrintf("New regs[13] is [%x]\n", status->regs[13]);
            break;
        case CMD_FINISH:
            LogPrintf("CMD_FINISH\n");
            if(!(status->r15_updated)) {
                status->regs[15] = status->regs[14];
                status->r15_updated = true;
            }
            break;
        case CMD_VFP_POP_X: 
            LogPrintf("CMD_VFP_POP_X\n");
            status->regs[13] += cmd.data * 8 + 4; // see EHABI
            break;
        case CMD_VFP_POP_D: 
            LogPrintf("CMD_VFP_POP_D\n");
            status->regs[13] += cmd.data * 8; // see EHABI
            break;
        case CMD_WREG_POP:
            LogPrintf("CMD_WREG_POP\n");
            status->regs[13] += cmd.data * 8;  
            break;
        case CMD_WCGR_POP:
            LogPrintf("CMD_WCGR_POP\n");
            for(i = 0; i < 4; ++i)
                if((cmd.data >> i) & 1) 
                    status->regs[13] += 4;
            break;
            
        case CMD_RESERVED:
            LogPrintf("CMD_RESERVED\n");
            CHECK_RET(false, RESERVED_UNW_COMMAND, exit_point, ret);
            break;

    }
exit_point:
    return ret;
}
